# 输出URDF路径
from os.path import dirname, join, abspath
import yaml

# 这里open的基路径是main.py
def filePath(URDF_NAME = None, ROBOT = False):
    if URDF_NAME == None:
        return 'ERROR: URDF_Name = None'
    # 根据Robot关键字的bool值，选取特定的init.yml文件
    if ROBOT:
        # 这里open的基路径是main.py
        with open(join(dirname(str(abspath(__file__))),'robot_init.yml'), 'r', encoding='utf-8') as f:
            init_config= yaml.load(f.read(), Loader=yaml.FullLoader)
    else:
        with open(join(dirname(str(abspath(__file__))),'init.yml'), 'r', encoding='utf-8') as f:
            init_config= yaml.load(f.read(), Loader=yaml.FullLoader)
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    dir_path = init_config['urdf_dir']
    # 这里__file__的路径是当前函数文件所在路径
    URDF_FILE_DIR_PATH = join(dirname(dirname(str(abspath(__file__)))), dir_path)
    URDF_FILE_PATH = join(URDF_FILE_DIR_PATH, str(URDF_NAME)+'.urdf')
    return URDF_FILE_PATH